/**
 *
 * File:   motor_control.c
 * Description: 电机控制
 * 使用方法
 * 1.freeRtos 一个独立线程 while 之前 Motor_Init()  初始化电机
 * 2.Motor_Run(rbus_parser)  根据RBUS数据进行电机运动控制(freeRtos里的while循环里运行每隔20MS读取SBUS数据，进行电机运动控制)
 *
 */

#include "motor_control.h"

// 全局电机对象定义
Motor left_front, right_front, left_back, right_back;

// 限制PWM值（静态函数，只在当前文件使用）
static int16_t constrain_PWM(int16_t value)
{
    if (value > MAX_PWM)
        return MAX_PWM;
    if (value < MIN_PWM)
        return MIN_PWM;
    return value;
}

// 限制数值范围
int16_t constrain_value(int16_t value, int16_t min_val, int16_t max_val)
{
    if (value > max_val)
        return max_val;
    if (value < min_val)
        return min_val;
    return value;
}

// 初始化电机
void Motor_Init(void)
{
    // 启动PWM定时器
    HAL_TIM_PWM_Start(LF_FORWARD_TIM, LF_FORWARD_CH);
    HAL_TIM_PWM_Start(LF_BACKWARD_TIM, LF_BACKWARD_CH);
    HAL_TIM_PWM_Start(RF_FORWARD_TIM, RF_FORWARD_CH);
    HAL_TIM_PWM_Start(RF_BACKWARD_TIM, RF_BACKWARD_CH);
    HAL_TIM_PWM_Start(LB_FORWARD_TIM, LB_FORWARD_CH);
    HAL_TIM_PWM_Start(LB_BACKWARD_TIM, LB_BACKWARD_CH);
    HAL_TIM_PWM_Start(RB_FORWARD_TIM, RB_FORWARD_CH);
    HAL_TIM_PWM_Start(RB_BACKWARD_TIM, RB_BACKWARD_CH);

    // 左前轮
    left_front.forward_tim = LF_FORWARD_TIM;
    left_front.forward_ch = LF_FORWARD_CH;
    left_front.backward_tim = LF_BACKWARD_TIM;
    left_front.backward_ch = LF_BACKWARD_CH;

    // 右前轮
    right_front.forward_tim = RF_FORWARD_TIM;
    right_front.forward_ch = RF_FORWARD_CH;
    right_front.backward_tim = RF_BACKWARD_TIM;
    right_front.backward_ch = RF_BACKWARD_CH;

    // 左后轮
    left_back.forward_tim = LB_FORWARD_TIM;
    left_back.forward_ch = LB_FORWARD_CH;
    left_back.backward_tim = LB_BACKWARD_TIM;
    left_back.backward_ch = LB_BACKWARD_CH;

    // 右后轮
    right_back.forward_tim = RB_FORWARD_TIM;
    right_back.forward_ch = RB_FORWARD_CH;
    right_back.backward_tim = RB_BACKWARD_TIM;
    right_back.backward_ch = RB_BACKWARD_CH;

    // 初始停止所有电机
    stop_all_motors();

    printf("Motor Initialization Complete\r\n");
}

void Motor_Run(RBUS_parser *rbus_parser)
{
    //printf("Rbus: Power:%3d Lost:%3d \r\n", rbus_parser->switch_power, rbus_parser->frame_lost);
    if (rbus_parser->switch_power == RBUS_SWITCH_ON)
    {
        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_4, GPIO_PIN_RESET);
    }
    else
    {
        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_4, GPIO_PIN_SET);
    }

    if (rbus_parser->frame_lost)    // 有摇杆丢失闪红灯
    {
        HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_3);
    }
    else
    {
        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_3, GPIO_PIN_SET);
    }

    if (rbus_parser->switch_power == RBUS_SWITCH_OFF)
    {
        // 刹车停止所有电机
        brake_all_motors();
        return;
    }

    if (rbus_parser->frame_lost == 1)
    {
        // 惯性停止所有电机
        stop_all_motors();
        return;
    }

    // 解算摇杆位置
    JoystickDir left_joystick, right_joystick;
    left_joystick.x = rbus_parser->left_x;
    left_joystick.y = rbus_parser->left_y;
    right_joystick.x = rbus_parser->right_x;
    right_joystick.y = rbus_parser->right_y;

    // 下发控制电机运动
    mecanum_drive(left_joystick, right_joystick);
}

// 设置单个电机速度
void set_motor_speed(Motor motor, int16_t speed)
{
    uint16_t pwm_value;

    if (speed > 0)
    {
        // 前进
        pwm_value = constrain_PWM(speed);
        __HAL_TIM_SET_COMPARE(motor.forward_tim, motor.forward_ch, pwm_value);
        __HAL_TIM_SET_COMPARE(motor.backward_tim, motor.backward_ch, 0);
    }
    else if (speed < 0)
    {
        // 后退
        pwm_value = constrain_PWM(-speed);
        __HAL_TIM_SET_COMPARE(motor.forward_tim, motor.forward_ch, 0);
        __HAL_TIM_SET_COMPARE(motor.backward_tim, motor.backward_ch, pwm_value);
    }
    else
    {
        // 停止
        __HAL_TIM_SET_COMPARE(motor.forward_tim, motor.forward_ch, 0);
        __HAL_TIM_SET_COMPARE(motor.backward_tim, motor.backward_ch, 0);
    }
}

// 麦克纳姆轮运动控制
void mecanum_drive(JoystickDir left_joy, JoystickDir right_joy)
{
    // 死区处理
    if (abs(left_joy.x) < 10)
        left_joy.x = 0;
    if (abs(left_joy.y) < 10)
        left_joy.y = 0;
    if (abs(right_joy.x) < 10)
        right_joy.x = 0;

    // 计算各方向分量（摇杆范围：-100 ~ +100）
    int16_t vx = left_joy.x;  // 左右平移
    int16_t vy = left_joy.y;  // 前后移动
    int16_t vr = right_joy.x; // 旋转

    // 麦克纳姆轮运动学公式
    int16_t lf_speed = vy + vx + vr; // 左前轮: 前+右+顺时
    int16_t rf_speed = vy - vx - vr; // 右前轮: 前-右-顺时
    int16_t lb_speed = vy - vx + vr; // 左后轮: 前-右+顺时
    int16_t rb_speed = vy + vx - vr; // 右后轮: 前+右-顺时

    // 限制速度在 -100 ~ +100 范围内
    lf_speed = constrain_value(lf_speed, -100, 100);
    rf_speed = constrain_value(rf_speed, -100, 100);
    lb_speed = constrain_value(lb_speed, -100, 100);
    rb_speed = constrain_value(rb_speed, -100, 100);

    // 设置电机速度（速度值直接作为PWM百分比）
    set_motor_speed(left_front, lf_speed);
    set_motor_speed(right_front, rf_speed);
    set_motor_speed(left_back, lb_speed);
    set_motor_speed(right_back, rb_speed);

    // 调试信息
    // printf("Mecanum - LF:%3d RF:%3d LB:%3d RB:%3d\r\n", lf_speed, rf_speed, lb_speed, rb_speed);
}

// 简单差速控制（适用于普通四轮小车）
void simple_drive(int16_t joy_x, int16_t joy_y)
{
    // 死区处理
    if (abs(joy_x) < 10)
        joy_x = 0;
    if (abs(joy_y) < 10)
        joy_y = 0;

    // 计算左右轮速度
    int16_t left_speed = joy_y + joy_x;
    int16_t right_speed = joy_y - joy_x;

    // 限制速度范围
    left_speed = constrain_value(left_speed, -100, 100);
    right_speed = constrain_value(right_speed, -100, 100);

    // 设置电机速度（前后轮同侧速度相同）
    set_motor_speed(left_front, left_speed);
    set_motor_speed(left_back, left_speed);
    set_motor_speed(right_front, right_speed);
    set_motor_speed(right_back, right_speed);

    // 调试信息
    printf("Simple - Left:%3d Right:%3d\r\n", left_speed, right_speed);
}

// 简单映射函数
int16_t map(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

// 停止所有电机(惯性)
void stop_all_motors(void)
{
    set_motor_speed(left_front, 0);
    set_motor_speed(right_front, 0);
    set_motor_speed(left_back, 0);
    set_motor_speed(right_back, 0);
    // printf("All motors stopped\r\n");
}

// 刹车函数 - 紧急制动
void brake_all_motors(void)
{
    // 同时给前进和后退通道PWM，产生制动效果
    __HAL_TIM_SET_COMPARE(LF_FORWARD_TIM, LF_FORWARD_CH, MAX_PWM);
    __HAL_TIM_SET_COMPARE(LF_BACKWARD_TIM, LF_BACKWARD_CH, MAX_PWM);

    __HAL_TIM_SET_COMPARE(RF_FORWARD_TIM, RF_FORWARD_CH, MAX_PWM);
    __HAL_TIM_SET_COMPARE(RF_BACKWARD_TIM, RF_BACKWARD_CH, MAX_PWM);

    __HAL_TIM_SET_COMPARE(LB_FORWARD_TIM, LB_FORWARD_CH, MAX_PWM);
    __HAL_TIM_SET_COMPARE(LB_BACKWARD_TIM, LB_BACKWARD_CH, MAX_PWM);

    __HAL_TIM_SET_COMPARE(RB_FORWARD_TIM, RB_FORWARD_CH, MAX_PWM);
    __HAL_TIM_SET_COMPARE(RB_BACKWARD_TIM, RB_BACKWARD_CH, MAX_PWM);

    // 制动后延时一段时间再完全停止
    osDelay(50);
    stop_all_motors();
}

// 测试单个电机
void test_motor(Motor motor, int16_t speed, const char *motor_name)
{
    printf("Testing %s motor: %d\r\n", motor_name, speed);
    set_motor_speed(motor, speed);
    HAL_Delay(2000);
    set_motor_speed(motor, 0);
    HAL_Delay(500);
}

// 示例使用函数
void example_usage(void)
{
    // 初始化电机
    Motor_Init();

    // 示例1: 测试单个电机
    printf("=== Motor Test ===\r\n");
    test_motor(left_front, 50, "Left Front");
    test_motor(right_front, 50, "Right Front");
    test_motor(left_back, 50, "Left Back");
    test_motor(right_back, 50, "Right Back");

    // 示例2: 使用摇杆控制
    printf("=== Joystick Control Example ===\r\n");
    JoystickDir left_joystick, right_joystick;

    // 前进
    left_joystick.x = 0;
    left_joystick.y = 80;
    right_joystick.x = 0;
    mecanum_drive(left_joystick, right_joystick);
    HAL_Delay(1000);

    // 右转
    left_joystick.x = 0;
    left_joystick.y = 0;
    right_joystick.x = 60;
    mecanum_drive(left_joystick, right_joystick);
    HAL_Delay(1000);

    // 停止
    stop_all_motors();
}

// 电机诊断函数
void motor_diagnostic(void)
{
    printf("=== Motor Diagnostic ===\r\n");
    printf("PWM Range: %d to %d\r\n", MIN_PWM, MAX_PWM);

    // 测试各方向
    JoystickDir test_left = {0, 100}; // 全速前进
    JoystickDir test_right = {0, 0};

    printf("Testing forward...\r\n");
    mecanum_drive(test_left, test_right);
    HAL_Delay(2000);

    test_left.y = -100; // 全速后退
    printf("Testing backward...\r\n");
    mecanum_drive(test_left, test_right);
    HAL_Delay(2000);

    stop_all_motors();
    printf("Diagnostic complete\r\n");
}

//  printf("BuzzerBeep\r\n");
// HAL_UART_Transmit_IT(&huart3, (uint8_t *)"BuzzerBeep\r\n", strlen("BuzzerBeep\r\n"));
// __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 70);    //左前轮前进
//__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 70);     //左前轮后退
//  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 70);  //右前轮前进
//__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 70);   //右前轮后退
//__HAL_TIM_SET_COMPARE(&htim9, TIM_CHANNEL_1, 70);   //左后轮前进
//  __HAL_TIM_SET_COMPARE(&htim9, TIM_CHANNEL_2, 70);//左后轮后退
//   __HAL_TIM_SET_COMPARE(&htim12, TIM_CHANNEL_1, 70);//右后轮前进
//  __HAL_TIM_SET_COMPARE(&htim12, TIM_CHANNEL_2, 70);  //右后轮后退
